﻿using System;
using System.Drawing.Drawing2D;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.IO.Ports;
using System.Threading;
using System.Text.RegularExpressions;
//using System.Windows.Forms.DataVisualization.Charting.Series;//
using ZedGraph;

namespace @interface
{
    public partial class Form1 : Form
    {
        Mutex mutex_read_serial = new Mutex();
        Mutex mutex_write_serial = new Mutex();
        SerialPort serial = new SerialPort();
        MotorPidCommands pid = new MotorPidCommands();
        MotorReadings sensorReadings = new MotorReadings();
        System.Timers.Timer chartTimer = new System.Timers.Timer();
        int tickStart = 0;
        bool debugMode = false;

        public Form1()
        {
            InitializeComponent();
            KeyPreview = true;
            KeyDown += new KeyEventHandler(Form1_KeyDown);
            //PreviewKeyDown += new PreviewKeyDownEventHandler(Form1_PreviewKeyDown);
            chartTimer.Elapsed += new System.Timers.ElapsedEventHandler(updateGraph);
            chartTimer.Interval = 50;
            chartTimer.Enabled = true;
            chartTimer.Start();
            trackBar_motor1_desiredRPM.Value = 100;
            trackBar_motor2_desiredRPM.Value = 100;
            trackBar_motor3_desiredRPM.Value = 100;

            trackBarText_motor1_desiredRPM.Text = trackBar_motor1_desiredRPM.Value.ToString();
            trackBarText_motor2_desiredRPM.Text = trackBar_motor2_desiredRPM.Value.ToString();
            trackBarText_motor3_desiredRPM.Text = trackBar_motor3_desiredRPM.Value.ToString();
        }

        void chartTimer_Elapsed(object sender, System.Timers.ElapsedEventArgs e)
        {
            
        }

        void Form1_KeyDown(object sender, KeyEventArgs e)
        {
            int motor1_duty_coefficient = trackBar_motor1_desiredRPM.Value;
            int motor2_duty_coefficient = trackBar_motor2_desiredRPM.Value;
            int motor3_duty_coefficient = trackBar_motor3_desiredRPM.Value;
            int desiredRPM = 0;

            

            if (serial.IsOpen)
            {
                switch (Convert.ToChar(e.KeyValue))
                {
                    case 'Q':
                        desiredRPM = 0;
                        break;
                    case 'W':
                        desiredRPM = 40;
                        break;
                    case 'E':
                        desiredRPM = 60;
                        break;
                    case 'R':
                        desiredRPM = 80;
                        break;
                    case 'T':
                        desiredRPM = 100;
                        break;
                    case 'Y':
                        desiredRPM = 130;
                        break;
                    case 'U':
                        desiredRPM = 160;
                        break;
                    case 'I':
                        desiredRPM = 190;
                        break;
                    case 'O':
                        desiredRPM = 220;
                        break;
                    case 'P':
                        desiredRPM = 255;
                        break;
                    default:
                        return;
                        
                }
                mutex_write_serial.WaitOne();
                serial.Write("S1" + ((int)(desiredRPM * (float)trackBar_motor1_desiredRPM.Value / trackBar_motor1_desiredRPM.Maximum)).ToString().PadLeft(3, '0'));
                serial.Write("S2" + ((int)(desiredRPM * (float)trackBar_motor2_desiredRPM.Value / trackBar_motor2_desiredRPM.Maximum)).ToString().PadLeft(3, '0'));
                serial.Write("S3" + ((int)(desiredRPM * (float)trackBar_motor3_desiredRPM.Value / trackBar_motor3_desiredRPM.Maximum)).ToString().PadLeft(3, '0'));
                mutex_write_serial.ReleaseMutex();
            }

        }

        private void Form1_Load(object sender, EventArgs e)
        {
            PopulateSerialList();

        }

        private void PopulateSerialList()
        {
            string[] ports = SerialPort.GetPortNames();
            foreach (var p in ports)
            {
                comboBox1.Items.Add(p); 
            }

            if (comboBox1.Items.Count > 0)
            {
                comboBox1.SelectedIndex = 0;
            }

        }

        private void button1_Click(object sender, EventArgs e)
        {
            serial.PortName = comboBox1.SelectedItem.ToString();
            serial.Open();
            serial.DataReceived += new SerialDataReceivedEventHandler(serial_DataReceived);
            initializeGraph();
        }

        void serial_DataReceived(object sender, SerialDataReceivedEventArgs e)
        {
            SerialPort sp = (SerialPort)sender;
            mutex_read_serial.WaitOne();
            
            //sensorReadings.buffer.Append(sp.ReadExisting().ToString());
            //sensorReadings.updatesFragments.Add(sp.ReadTo("|").ToString());

            string txt;
            
            //txt = sp.ReadExisting();
            txt = sp.ReadTo("|");
            Console.WriteLine(txt);
            sensorReadings.parseCommand(txt);
            updateUiData();
            mutex_read_serial.ReleaseMutex();
        }
        private void updateUiData()
        {
            text_motor1_ki.Invoke(new Action(delegate() { text_motor1_ki.Text = sensorReadings.ki_motor1; }));
            text_motor1_kp.Invoke(new Action(delegate() { text_motor1_kp.Text = sensorReadings.kp_motor1; }));
            text_motor1_kd.Invoke(new Action(delegate() { text_motor1_kd.Text = sensorReadings.kd_motor1; }));

            text_motor1_duty.Invoke(new Action(delegate() { text_motor1_duty.Text = sensorReadings.dutyCycle_motor1; }));
            text_motor1_desiredRPM.Invoke(new Action(delegate() { text_motor1_desiredRPM.Text = sensorReadings.reportedSetpoint_motor1; }));
            text_motor1_actualRPM.Invoke(new Action(delegate() { text_motor1_actualRPM.Text = sensorReadings.currentRPM_motor1; }));

            text_motor1_current.Invoke(new Action(delegate() { text_motor1_current.Text = sensorReadings.current_motor1; }));
            text_motor1_temperature.Invoke(new Action(delegate() { text_motor1_temperature.Text = sensorReadings.temp_motor1; }));
            text_motor1_speedCoefficient.Invoke(new Action(delegate() { text_motor1_speedCoefficient.Text = sensorReadings.speedCoefficient_motor1; }));



            text_motor2_ki.Invoke(new Action(delegate() { text_motor2_ki.Text = sensorReadings.ki_motor2; }));
            text_motor2_kp.Invoke(new Action(delegate() { text_motor2_kp.Text = sensorReadings.kp_motor2; }));
            text_motor2_kd.Invoke(new Action(delegate() { text_motor2_kd.Text = sensorReadings.kd_motor2; }));
                      
                      
            text_motor2_duty.Invoke(new Action(delegate() { text_motor2_duty.Text = sensorReadings.dutyCycle_motor2; }));
            text_motor2_desiredRPM.Invoke(new Action(delegate() { text_motor2_desiredRPM.Text = sensorReadings.reportedSetpoint_motor2; }));
            text_motor2_actualRPM.Invoke(new Action(delegate() { text_motor2_actualRPM.Text = sensorReadings.currentRPM_motor2; }));
                      
            text_motor2_current.Invoke(new Action(delegate() { text_motor2_current.Text = sensorReadings.current_motor2; }));
            text_motor2_temperature.Invoke(new Action(delegate() { text_motor2_temperature.Text = sensorReadings.temp_motor2; }));
            text_motor2_speedCoefficient.Invoke(new Action(delegate() { text_motor2_speedCoefficient.Text = sensorReadings.speedCoefficient_motor2; }));




            text_motor3_ki.Invoke(new Action(delegate() { text_motor3_ki.Text = sensorReadings.ki_motor3; }));
            text_motor3_kp.Invoke(new Action(delegate() { text_motor3_kp.Text = sensorReadings.kp_motor3; }));
            text_motor3_kd.Invoke(new Action(delegate() { text_motor3_kd.Text = sensorReadings.kd_motor3; }));
                      
            text_motor3_duty.Invoke(new Action(delegate() { text_motor3_duty.Text = sensorReadings.dutyCycle_motor3; }));
            text_motor3_desiredRPM.Invoke(new Action(delegate() { text_motor3_desiredRPM.Text = sensorReadings.reportedSetpoint_motor3; }));
            text_motor3_actualRPM.Invoke(new Action(delegate() { text_motor3_actualRPM.Text = sensorReadings.currentRPM_motor3; }));
                      
            text_motor3_current.Invoke(new Action(delegate() { text_motor3_current.Text = sensorReadings.current_motor3; }));
            text_motor3_temperature.Invoke(new Action(delegate() { text_motor3_temperature.Text = sensorReadings.temp_motor3; }));

            
          
            text_motor3_speedCoefficient.Invoke(new Action(delegate() { text_motor3_speedCoefficient.Text = sensorReadings.speedCoefficient_motor3; }));
            
            text_direction.Invoke(new Action(delegate() { text_direction.Text = sensorReadings.direction; }));
        }

        private void button2_Click(object sender, EventArgs e)
        {
            serial.Close();
        }


        private void Form1_KeyPress(object sender, KeyPressEventArgs e)
        {
        }

        private void Form1_KeyUp(object sender, KeyEventArgs e)
        {



        }

      

        private void button4_Click(object sender, EventArgs e)
        {
            //changing ki
            string temp = inputText_motor1_kd.Text;
            double dummy;
            if (double.TryParse(temp, out dummy))
            {
                if (dummy < 100)
                {
                    dummy = Math.Round(dummy, 1, MidpointRounding.AwayFromZero);
                    int final_dummy = (int)(dummy * 10);
                    string p_value = final_dummy.ToString();

                    if (p_value.Length == 0)
                    {

                        return;
                    }
                    else if (p_value.Length == 1)
                    {
                        p_value = "00" + p_value;

                    }
                    else if (p_value.Length == 2)
                    {
                        p_value = "0" + p_value;
                    }
                    mutex_write_serial.WaitOne();
                    serial.Write("I" + p_value);
                    mutex_write_serial.ReleaseMutex();
                }
            }
        }

        private void button5_Click(object sender, EventArgs e)
        {
            //changing kd
            string temp = text_motor1_duty.Text;
            double dummy;
            if (double.TryParse(temp, out dummy))
            {
                if (dummy < 100)
                {
                    dummy = Math.Round(dummy, 1, MidpointRounding.AwayFromZero);
                    int final_dummy = (int)(dummy * 10);
                    string p_value = final_dummy.ToString();

                    if (p_value.Length == 0)
                    {
                        return;
                    }
                    else if (p_value.Length == 1)
                    {
                        p_value = "00" + p_value;

                    }
                    else if (p_value.Length == 2)
                    {
                        p_value = "0" + p_value;
                    }
                    mutex_write_serial.WaitOne();
                    serial.Write("D" + p_value);
                    mutex_write_serial.ReleaseMutex();
                }
            }
        }

        private void updateGraph(object sender, EventArgs e)
        {
            double time = (Environment.TickCount - tickStart) / 1000.0;
            if (zedGraphControl1.GraphPane.CurveList.Count <= 0)
                return;

            LineItem motor1_RPM_line =  zedGraphControl1.GraphPane.CurveList[0] as LineItem;
            LineItem motor2_RPM_line = zedGraphControl1.GraphPane.CurveList[1] as LineItem;
            LineItem motor3_RPM_line = zedGraphControl1.GraphPane.CurveList[2] as LineItem;

            LineItem motor1_duty_line = zedGraphControl1.GraphPane.CurveList[3] as LineItem;
            LineItem motor2_duty_line = zedGraphControl1.GraphPane.CurveList[4] as LineItem;
            LineItem motor3_duty_line = zedGraphControl1.GraphPane.CurveList[5] as LineItem;

            LineItem motor1_desiredRPM_line = zedGraphControl1.GraphPane.CurveList[6] as LineItem;
            LineItem motor2_desiredRPM_line = zedGraphControl1.GraphPane.CurveList[7] as LineItem;
            LineItem motor3_desiredRPM_line = zedGraphControl1.GraphPane.CurveList[8] as LineItem;

            // // 
            IPointListEdit motor1_RPM_points = motor1_RPM_line.Points as IPointListEdit;
            IPointListEdit motor2_RPM_points = motor2_RPM_line.Points as IPointListEdit;
            IPointListEdit motor3_RPM_points = motor3_RPM_line.Points as IPointListEdit;
            
            IPointListEdit motor1_duty_points =  motor1_duty_line.Points as IPointListEdit;
            IPointListEdit motor2_duty_points =  motor2_duty_line.Points as IPointListEdit;
            IPointListEdit motor3_duty_points =  motor3_duty_line.Points as IPointListEdit;

            IPointListEdit motor1_desiredRPM_points = motor1_desiredRPM_line.Points as IPointListEdit;
            IPointListEdit motor2_desiredRPM_points = motor2_desiredRPM_line.Points as IPointListEdit;
            IPointListEdit motor3_desiredRPM_points = motor3_desiredRPM_line.Points as IPointListEdit;

            
            // // 
            double dummy;

            //double motor1_RPM;
            //double motor2_RPM;
            //double motor3_RPM;
            
            //double motor1_desiredRPM;
            //double motor2_desiredRPM;
            //double motor3_desiredRPM;
            
            //double motor1_duty;
            //double motor2_duty;
            //double motor3_duty;

            if (checkbox_motor1_RPM.Checked && (double.TryParse(sensorReadings.currentRPM_motor1, out dummy)))
            {
                motor1_RPM_points.Add(time, dummy);
            }

            if (checkbox_motor2_RPM.Checked && (double.TryParse(sensorReadings.currentRPM_motor2, out dummy)))
            {
                motor2_RPM_points.Add(time, dummy);
            }

            if (checkbox_motor3_RPM.Checked &&  (double.TryParse(sensorReadings.currentRPM_motor3, out dummy)))
            {
                motor3_RPM_points.Add(time, dummy);
            }



            if (checkbox_motor1_desiredRPM.Checked && (double.TryParse(sensorReadings.reportedSetpoint_motor1, out dummy)))
            {
                motor1_desiredRPM_points.Add(time, dummy);
            }

            if (checkbox_motor2_desiredRPM.Checked && (double.TryParse(sensorReadings.reportedSetpoint_motor2, out dummy)))
            {
                motor2_desiredRPM_points.Add(time, dummy);
            }

            if (checkbox_motor3_desiredRPM.Checked && (double.TryParse(sensorReadings.reportedSetpoint_motor3, out dummy)))
            {
                motor3_desiredRPM_points.Add(time, dummy);
            }



            if (checkbox_motor1_duty.Checked && (double.TryParse(sensorReadings.dutyCycle_motor1, out dummy)))
            {
                motor1_duty_points.Add(time, dummy);
            }

            if (checkbox_motor2_duty.Checked && (double.TryParse(sensorReadings.dutyCycle_motor2, out dummy)))
            {
                motor2_duty_points.Add(time, dummy);
            }

            if (checkbox_motor3_duty.Checked &&  (double.TryParse(sensorReadings.dutyCycle_motor3, out dummy)))
            {
                motor3_duty_points.Add(time, dummy);
            }
            
            // Keep the X scale at a rolling 30 second interval, with one
            // major step between the max X value and the end of the axis
            Scale xScale = zedGraphControl1.GraphPane.XAxis.Scale;
            if (time > xScale.Max - xScale.MajorStep)
            {
                xScale.Max = time + xScale.MajorStep;
                xScale.Min = xScale.Max - 30.0;
            }

            // Make sure the Y axis is rescaled to accommodate actual data
            zedGraphControl1.AxisChange();
            // Force a redraw
            zedGraphControl1.Invalidate();

        }

        private void initializeGraph()
        {
            GraphPane myPane = zedGraphControl1.GraphPane;
            myPane.Title.Text = "RPM, Target RPM and Duty vs Time";
            myPane.XAxis.Title.Text = "Time, Seconds";
            myPane.YAxis.Title.Text = "RPM, Target RPM, Duty";

            // Save 1200 points.  At 50 ms sample rate, this is one minute
            // The RollingPointPairList is an efficient storage class that always
            // keeps a rolling set of point data without needing to shift any data values
            RollingPointPairList motor1_RPM_point = new RollingPointPairList(1200);
            RollingPointPairList motor2_RPM_point = new RollingPointPairList(1200);
            RollingPointPairList motor3_RPM_point = new RollingPointPairList(1200);

            RollingPointPairList motor1_duty_point = new RollingPointPairList(1200);
            RollingPointPairList motor2_duty_point = new RollingPointPairList(1200);
            RollingPointPairList motor3_duty_point = new RollingPointPairList(1200);

            RollingPointPairList motor1_desiredRPM_point = new RollingPointPairList(1200);
            RollingPointPairList motor2_desiredRPM_point = new RollingPointPairList(1200);
            RollingPointPairList motor3_desiredRPM_point = new RollingPointPairList(1200);

            
            // Initially, a curve is added with no data points (list is empty)

            LineItem motor1_RPM_line = myPane.AddCurve("Motor1 RPM", motor1_RPM_point, Color.Blue, SymbolType.None);
            //motor1_RPM_line.Line.Style = DashStyle.Custom;
            //motor1_RPM_line.Line.Width = 3;
            //motor1_RPM_line.Line.DashOn = 5;
            //motor1_RPM_line.Line.DashOff = 10;

            LineItem motor2_RPM_line = myPane.AddCurve("Motor2 RPM", motor2_RPM_point, Color.Violet, SymbolType.None);
            //motor2_RPM_line.Line.Style = DashStyle.Custom;
            //motor2_RPM_line.Line.Width = 3;
            //motor2_RPM_line.Line.DashOn = 5;
            //motor2_RPM_line.Line.DashOff = 10;

            LineItem motor3_RPM_line = myPane.AddCurve("Motor3 RPM", motor3_RPM_point, Color.Aquamarine, SymbolType.None);
            //motor3_RPM_line.Line.Style = DashStyle.Custom;
            //motor3_RPM_line.Line.Width = 3;
            //motor3_RPM_line.Line.DashOn = 5;
            //motor3_RPM_line.Line.DashOff = 10;

            // // // 
            LineItem motor1_duty_line = myPane.AddCurve("Motor1 Duty", motor1_duty_point, Color.Orange, SymbolType.None);
            //motor1_duty_line.Line.Style = DashStyle.Custom;
            //motor1_duty_line.Line.Width = 3;
            //motor1_duty_line.Line.DashOn = 10;
            //motor1_duty_line.Line.DashOff = 5;

            LineItem motor2_duty_line = myPane.AddCurve("Motor2 Duty", motor2_duty_point, Color.OrangeRed, SymbolType.None);
            //motor2_duty_line.Line.Style = DashStyle.Custom;
            //motor2_duty_line.Line.Width = 3;
            //motor2_duty_line.Line.DashOn = 10;
            //motor2_duty_line.Line.DashOff = 5;

            LineItem motor3_duty_line = myPane.AddCurve("Motor3 Duty", motor3_duty_point, Color.DarkOrange, SymbolType.None);
            //motor3_duty_line.Line.Style = DashStyle.Custom;
            //motor3_duty_line.Line.Width = 3;
            //motor3_duty_line.Line.DashOn = 10;
            //motor3_duty_line.Line.DashOff = 5;

            
            LineItem motor1_desiredRPM_line = myPane.AddCurve("Motor1 Target RPM", motor1_desiredRPM_point, Color.Green, SymbolType.None);
            LineItem motor2_desiredRPM_line = myPane.AddCurve("Motor2 Target RPM", motor2_desiredRPM_point, Color.DarkGreen, SymbolType.None);
            LineItem motor3_desiredRPM_line = myPane.AddCurve("Motor3 Target RPM", motor3_desiredRPM_point, Color.ForestGreen, SymbolType.None);
                       
            // Sample at 50ms intervals
            chartTimer.Interval = 50;
            chartTimer.Enabled = true;
            chartTimer.Start();

            // Just manually control the X axis range so it scrolls continuously
            // instead of discrete step-sized jumps
            myPane.XAxis.Scale.Min = 0;
            myPane.XAxis.Scale.Max = 30;
            myPane.XAxis.Scale.MinorStep = 1;
            myPane.XAxis.Scale.MajorStep = 5;

            // Scale the axes
            zedGraphControl1.AxisChange();

            // Save the beginning time for reference
            tickStart = Environment.TickCount;
        }


        private void textBox11_TextChanged(object sender, EventArgs e)
        {
            
        }

        private void button6_Click(object sender, EventArgs e)
        {
            mutex_write_serial.WaitOne();
            serial.Write("t101");
            mutex_write_serial.ReleaseMutex();
        }

   

        private void textBox21_TextChanged(object sender, EventArgs e)
        {

        }

        
        private void button_motor1_kp_Click(object sender, EventArgs e)
        {
            string temp = inputText_moto1_kp.Text;
            double dummy;
            if (double.TryParse(temp, out dummy))
            {
                if (dummy < 100)
                {
                    dummy = Math.Round(dummy, 1, MidpointRounding.AwayFromZero);
                    int final_dummy = (int)(dummy * 10);
                    string p_value = final_dummy.ToString();

                    if (p_value.Length == 0)
                    {

                        return;
                    }
                    else if (p_value.Length == 1)
                    {
                        p_value = "00" + p_value;

                    }
                    else if (p_value.Length == 2)
                    {
                        p_value = "0" + p_value;
                    }
                    mutex_write_serial.WaitOne();
                    serial.Write("P1" + p_value);
                    mutex_write_serial.ReleaseMutex();
                }
            }
            
        }

        private void button_motor2_kp_Click(object sender, EventArgs e)
        {
            string temp = inputText_moto2_kp.Text;
            double dummy;
            if (double.TryParse(temp, out dummy))
            {
                if (dummy < 100)
                {
                    dummy = Math.Round(dummy, 1, MidpointRounding.AwayFromZero);
                    int final_dummy = (int)(dummy * 10);
                    string p_value = final_dummy.ToString();

                    if (p_value.Length == 0)
                    {

                        return;
                    }
                    else if (p_value.Length == 1)
                    {
                        p_value = "00" + p_value;

                    }
                    else if (p_value.Length == 2)
                    {
                        p_value = "0" + p_value;
                    }
                    mutex_write_serial.WaitOne();
                    serial.Write("P2" + p_value);
                    mutex_write_serial.ReleaseMutex();
                }
            }
            
        }

        private void button_motor3_kp_Click(object sender, EventArgs e)
        {
            string temp = inputText_moto3_kp.Text;
            double dummy;
            if (double.TryParse(temp, out dummy))
            {
                if (dummy < 100)
                {
                    dummy = Math.Round(dummy, 1, MidpointRounding.AwayFromZero);
                    int final_dummy = (int)(dummy * 10);
                    string p_value = final_dummy.ToString();

                    if (p_value.Length == 0)
                    {

                        return;
                    }
                    else if (p_value.Length == 1)
                    {
                        p_value = "00" + p_value;

                    }
                    else if (p_value.Length == 2)
                    {
                        p_value = "0" + p_value;
                    }
                    mutex_write_serial.WaitOne();
                    serial.Write("P3" + p_value);
                    mutex_write_serial.ReleaseMutex();
                }
            }
            
        }

        private void button_motor1_ki_Click(object sender, EventArgs e)
        {
            string temp = inputText_motor1_ki.Text;
            double dummy;
            if (double.TryParse(temp, out dummy))
            {
                if (dummy < 100)
                {
                    dummy = Math.Round(dummy, 1, MidpointRounding.AwayFromZero);
                    int final_dummy = (int)(dummy * 10);
                    string p_value = final_dummy.ToString();

                    if (p_value.Length == 0)
                    {

                        return;
                    }
                    else if (p_value.Length == 1)
                    {
                        p_value = "00" + p_value;

                    }
                    else if (p_value.Length == 2)
                    {
                        p_value = "0" + p_value;
                    }
                    mutex_write_serial.WaitOne();
                    serial.Write("I1" + p_value);
                    mutex_write_serial.ReleaseMutex();
                }
            }
        }

        private void button_motor2_ki_Click(object sender, EventArgs e)
        {
            string temp = inputText_motor2_ki.Text;
            double dummy;
            if (double.TryParse(temp, out dummy))
            {
                if (dummy < 100)
                {
                    dummy = Math.Round(dummy, 1, MidpointRounding.AwayFromZero);
                    int final_dummy = (int)(dummy * 10);
                    string p_value = final_dummy.ToString();

                    if (p_value.Length == 0)
                    {

                        return;
                    }
                    else if (p_value.Length == 1)
                    {
                        p_value = "00" + p_value;

                    }
                    else if (p_value.Length == 2)
                    {
                        p_value = "0" + p_value;
                    }
                    mutex_write_serial.WaitOne();
                    serial.Write("I2" + p_value);
                    mutex_write_serial.ReleaseMutex();
                }
            }
        }

        private void button_motor3_ki_Click(object sender, EventArgs e)
        {
            string temp = inputText_motor3_ki.Text;
            double dummy;
            if (double.TryParse(temp, out dummy))
            {
                if (dummy < 100)
                {
                    dummy = Math.Round(dummy, 1, MidpointRounding.AwayFromZero);
                    int final_dummy = (int)(dummy * 10);
                    string p_value = final_dummy.ToString();

                    if (p_value.Length == 0)
                    {

                        return;
                    }
                    else if (p_value.Length == 1)
                    {
                        p_value = "00" + p_value;

                    }
                    else if (p_value.Length == 2)
                    {
                        p_value = "0" + p_value;
                    }
                    mutex_write_serial.WaitOne();
                    serial.Write("I3" + p_value);
                    mutex_write_serial.ReleaseMutex();
                }
            }
        }

        private void button_motor1_kd_Click(object sender, EventArgs e)
        {
            string temp = inputText_motor1_kd.Text;
            double dummy;
            if (double.TryParse(temp, out dummy))
            {
                if (dummy < 100)
                {
                    dummy = Math.Round(dummy, 1, MidpointRounding.AwayFromZero);
                    int final_dummy = (int)(dummy * 10);
                    string p_value = final_dummy.ToString();

                    if (p_value.Length == 0)
                    {

                        return;
                    }
                    else if (p_value.Length == 1)
                    {
                        p_value = "00" + p_value;

                    }
                    else if (p_value.Length == 2)
                    {
                        p_value = "0" + p_value;
                    }
                    mutex_write_serial.WaitOne();
                    serial.Write("D1" + p_value);
                    mutex_write_serial.ReleaseMutex();
                }
            }
        }

        private void button_motor2_kd_Click(object sender, EventArgs e)
        {
            string temp = inputText_motor2_kd.Text;
            double dummy;
            if (double.TryParse(temp, out dummy))
            {
                if (dummy < 100)
                {
                    dummy = Math.Round(dummy, 1, MidpointRounding.AwayFromZero);
                    int final_dummy = (int)(dummy * 10);
                    string p_value = final_dummy.ToString();

                    if (p_value.Length == 0)
                    {

                        return;
                    }
                    else if (p_value.Length == 1)
                    {
                        p_value = "00" + p_value;

                    }
                    else if (p_value.Length == 2)
                    {
                        p_value = "0" + p_value;
                    }
                    mutex_write_serial.WaitOne();
                    serial.Write("D2" + p_value);
                    mutex_write_serial.ReleaseMutex();
                }
            }
        }

        private void button_motor3_kd_Click(object sender, EventArgs e)
        {
            string temp = inputText_motor3_kd.Text;
            double dummy;
            if (double.TryParse(temp, out dummy))
            {
                if (dummy < 100)
                {
                    dummy = Math.Round(dummy, 1, MidpointRounding.AwayFromZero);
                    int final_dummy = (int)(dummy * 10);
                    string p_value = final_dummy.ToString();

                    if (p_value.Length == 0)
                    {

                        return;
                    }
                    else if (p_value.Length == 1)
                    {
                        p_value = "00" + p_value;

                    }
                    else if (p_value.Length == 2)
                    {
                        p_value = "0" + p_value;
                    }
                    mutex_write_serial.WaitOne();
                    serial.Write("D3" + p_value);
                    mutex_write_serial.ReleaseMutex();
                }
            }
        }

        private void button3_Click(object sender, EventArgs e)
        {
            initializeGraph();
        }

        
        private void trackBar_motor3_desiredRPM_Scroll(object sender, EventArgs e)
        {
            trackBarText_motor3_desiredRPM.Text = trackBar_motor3_desiredRPM.Value.ToString();
        }

        private void trackBar_motor1_desiredRPM_Scroll(object sender, EventArgs e)
        {
            trackBarText_motor1_desiredRPM.Text = trackBar_motor1_desiredRPM.Value.ToString();
        }

        private void trackBar_motor2_desiredRPM_Scroll(object sender, EventArgs e)
        {
            trackBarText_motor2_desiredRPM.Text = trackBar_motor2_desiredRPM.Value.ToString();
        }

        private void label10_Click(object sender, EventArgs e)
        {

        }

        private void button_motor1_speedCoefficient_Click(object sender, EventArgs e)
        {
            string temp = inputText_motor1_speedCoefficient.Text;
            double dummy;
            if (double.TryParse(temp, out dummy))
            {
                if (dummy < 100)
                {
                    dummy = Math.Round(dummy, 1, MidpointRounding.AwayFromZero);
                    int final_dummy = (int)(dummy * 10);
                    string p_value = final_dummy.ToString();

                    if (p_value.Length == 0)
                    {
                        return;
                    }
                    else if (p_value.Length == 1)
                    {
                        p_value = "00" + p_value;

                    }
                    else if (p_value.Length == 2)
                    {
                        p_value = "0" + p_value;
                    }
                    mutex_write_serial.WaitOne();
                    serial.Write("E1" + p_value);
                    mutex_write_serial.ReleaseMutex();
                }
            }

        }

        private void button_motor2_speedCoefficient_Click(object sender, EventArgs e)
        {
            string temp = inputText_motor2_speedCoefficient.Text;
            double dummy;
            if (double.TryParse(temp, out dummy))
            {
                if (dummy < 100)
                {
                    dummy = Math.Round(dummy, 1, MidpointRounding.AwayFromZero);
                    int final_dummy = (int)(dummy * 10);
                    string p_value = final_dummy.ToString();

                    if (p_value.Length == 0)
                    {
                        return;
                    }
                    else if (p_value.Length == 1)
                    {
                        p_value = "00" + p_value;

                    }
                    else if (p_value.Length == 2)
                    {
                        p_value = "0" + p_value;
                    }
                    mutex_write_serial.WaitOne();
                    serial.Write("E2" + p_value);
                    mutex_write_serial.ReleaseMutex();
                }
            }
        }

        private void button_motor3_speedCoefficient_Click(object sender, EventArgs e)
        {
            string temp = inputText_motor3_speedCoefficient.Text;
            double dummy;
            if (double.TryParse(temp, out dummy))
            {
                if (dummy < 100)
                {
                    dummy = Math.Round(dummy, 1, MidpointRounding.AwayFromZero);
                    int final_dummy = (int)(dummy * 10);
                    string p_value = final_dummy.ToString();

                    if (p_value.Length == 0)
                    {
                        return;
                    }
                    else if (p_value.Length == 1)
                    {
                        p_value = "00" + p_value;

                    }
                    else if (p_value.Length == 2)
                    {
                        p_value = "0" + p_value;
                    }
                    mutex_write_serial.WaitOne();
                    serial.Write("E3" + p_value);
                    mutex_write_serial.ReleaseMutex();
                }
            }
        }

        private void textBox2_TextChanged(object sender, EventArgs e)
        {

        }

        private void button_debug_toggle_Click(object sender, EventArgs e)
        {
            if (debugMode == false)
            {
                debugMode = true;
                button_debug_toggle.Text = "Turn Debug Off";
            }
            else
            {
                debugMode = false;
                button_debug_toggle.Text = "Turn Debug On";
            }
        }
                
    }

    public class MotorReadings
    {
        public string dutyCycle_motor1;
        public string dutyCycle_motor2;
        public string dutyCycle_motor3;
        
        public string currentRPM_motor1;
        public string currentRPM_motor2;
        public string currentRPM_motor3;
        
        public string reportedSetpoint_motor1;
        public string reportedSetpoint_motor2;
        public string reportedSetpoint_motor3;

        public string kp_motor1;
        public string kp_motor2;
        public string kp_motor3;
        
        public string ki_motor1;
        public string ki_motor2;
        public string ki_motor3;

        public string kd_motor1;
        public string kd_motor2;
        public string kd_motor3;

        public string current_motor1;
        public string current_motor2;
        public string current_motor3;

        public string temp_motor1;
        public string temp_motor2;
        public string temp_motor3;

        public string raw_delta_motor1;
        public string raw_delta_motor2;
        public string raw_delta_motor3;

        public string speedCoefficient_motor1;
        public string speedCoefficient_motor2;
        public string speedCoefficient_motor3;

        public string direction;

        public StringBuilder buffer = new StringBuilder();
        public List<string> updatesFragments = new List<string>();
        List<double> tt = new List<double>();
        public void parseCommand(string command)
        {
            Int32 motorNumvalue;
            int motorNum;
            Int32 someValue;
            double value;

               if (!(command.Length > 2))
                {
                    return;
                }

                string commandValue = command.Substring(2).ToString();

                if (Int32.TryParse(command[1].ToString(), out motorNumvalue))
                {
                    motorNum = motorNumvalue;
                }
                else
                {
                    motorNum = 99;
                }

                switch (command[0])
                {
                    case 'E':
                        if (double.TryParse(commandValue, out value))
                        {
                            switch (motorNum)
                            {
                                case 1:
                                    speedCoefficient_motor1 = (value / 10).ToString();
                                    break;
                                case 2:
                                    speedCoefficient_motor2 = (value / 10).ToString();
                                    break;
                                case 3:
                                    speedCoefficient_motor3 = (value / 10).ToString();
                                    break;
                                default:
                                    break;
                            }
                        }
                        break;
                    case 't':
                        if (Int32.TryParse(commandValue, out someValue))
                        {
                            if (someValue == 111)
                            {
                                direction = "Forward";
                            }
                            else if (someValue == 000)
                            {
                                direction = "Reverse";
                            }

                        }
                        break;
                    case 'D':
                        {
                            if (double.TryParse(commandValue, out value))
                            {
                                switch (motorNum)
                                {
                                    case 1:
                                        dutyCycle_motor1 = ((int)(value / 255 * 100)).ToString();
                                        break;
                                    case 2:
                                        dutyCycle_motor2 = ((int)(value / 255 * 100)).ToString();
                                        break;
                                    case 3:
                                        dutyCycle_motor3 = ((int)(value / 255 * 100)).ToString();
                                        break;
                                    default:
                                        break;
                                }
                            }
                        }
                        break;
                    case 'R':
                        {
                            if (double.TryParse(commandValue, out value))
                            {
                                switch (motorNum)
                                {
                                    case 1:
                                        currentRPM_motor1 = value.ToString();
                                        break;
                                    case 2:
                                        currentRPM_motor2 = value.ToString();
                                        break;
                                    case 3:
                                        currentRPM_motor3 = value.ToString();
                                        break;
                                    default:
                                        break;
                                }

                            }
                        }
                        break;
                    case 'S':
                        {
                            if (double.TryParse(commandValue, out value))
                            {
                                switch (motorNum)
                                {
                                    case 1:
                                        reportedSetpoint_motor1 = value.ToString();
                                        break;
                                    case 2:
                                        reportedSetpoint_motor2 = value.ToString();
                                        break;
                                    case 3:
                                        reportedSetpoint_motor3 = value.ToString();
                                        break;
                                    default:
                                        break;
                                }
                            }
                        }
                        break;
                    case 'p':
                        if (double.TryParse(commandValue, out value))
                        {
                            switch (motorNum)
                            {
                                case 1:
                                    kp_motor1 = value.ToString();
                                    break;
                                case 2:
                                    kp_motor2 = value.ToString();
                                    break;
                                case 3:
                                    kp_motor3 = value.ToString();
                                    break;
                                default:
                                    break;
                            }
                        }
                        break;
                    case 'i':
                        if (double.TryParse(commandValue, out value))
                        {
                            switch (motorNum)
                            {
                                case 1:
                                    ki_motor1 = value.ToString();
                                    break;
                                case 2:
                                    ki_motor2 = value.ToString();
                                    break;
                                case 3:
                                    ki_motor3 = value.ToString();
                                    break;
                                default:
                                    break;
                            }
                        }
                        break;
                    case 'd':
                        if (double.TryParse(commandValue, out value))
                        {
                            switch (motorNum)
                            {
                                case 1:
                                    kd_motor1 = value.ToString();
                                    break;
                                case 2:
                                    kd_motor2 = value.ToString();
                                    break;
                                case 3:
                                    kd_motor3 = value.ToString();
                                    break;
                                default:
                                    break;
                            }
                        }
                        break;
                    case 'C':
                        if (double.TryParse(commandValue, out value))
                        {
                            switch (motorNum)
                            {
                                case 1:
                                    current_motor1 = value.ToString();
                                    break;
                                case 2:
                                    current_motor2 = value.ToString();
                                    break;
                                case 3:
                                    current_motor3 = value.ToString();
                                    break;
                                default:
                                    break;
                            }
                        }
                        break;
                    case 'T':
                        if (double.TryParse(commandValue, out value))
                        {
                            switch (motorNum)
                            {
                                case 1:
                                    temp_motor1 = value.ToString();
                                    break;
                                case 2:
                                    temp_motor2 = value.ToString();
                                    break;
                                case 3:
                                    temp_motor3 = value.ToString();
                                    break;
                                default:
                                    break;
                            }
                        }
                        break;
                    case 'W':
                        if (double.TryParse(commandValue, out value))
                        {
                            switch (motorNum)
                            {
                                case 1:
                                    raw_delta_motor1 = value.ToString();
                                    break;
                                case 2:
                                    raw_delta_motor2 = value.ToString();
                                    break;
                                case 3:
                                    raw_delta_motor3 = value.ToString();
                                    break;
                                default:
                                    break;
                            }
                        }
                        break;
                    default:
                        break;
                }


            
        }
    }

    public class MotorPidCommands
    {
        public string kp;
        public string ki;
        public string kd;
        
        public string setpoint;
    }

        
}


